//#include <iostream>
//#include <random>
//#include <chrono>
//#include <vector>
//#include "KalmanFilter.h"
//#include <fstream>
//#include <cmath>
//
//int main() {
//    double Gaussian_noise(double, double);
//    std::cout << "In" << std::endl;
//    KalmanFilter kf;
//
////    int n = 2, l = 0, m = 2;
////
////    Eigen::MatrixXd p0 = Eigen::MatrixXd(n, n);
////    p0 << 1.0, 0,
////            0, 1.0;
////
////    Eigen::MatrixXd x_Hat0 = Eigen::MatrixXd(n, 1);
////    x_Hat0 << 0.0,
////            1.0;
////
////    Eigen::MatrixXd inputQ = Eigen::MatrixXd(n, n);
////    inputQ << 1.0, 0,
////            0, 1.0;
////
////    Eigen::MatrixXd inputR = Eigen::MatrixXd(n, n);
////    inputR << 1, 0,
////            0, 1;
////
////    Eigen::MatrixXd inputA = Eigen::MatrixXd(n, n);
////    inputA << 1.0, 1.0,
////            0, 1.0;
////
////    Eigen::MatrixXd inputH = Eigen::MatrixXd(m, n);
////    inputH << 1.0, 0,
////            0, 1.0;
////
////    Eigen::MatrixXd Kk0 = Eigen::MatrixXd(n, n);
////
////    Eigen::MatrixXd inputI = Eigen::MatrixXd::Identity(n, n);
////
////    kf.Init(p0, x_Hat0, inputQ, inputR, inputA, inputH, inputI, Kk0);
////
////    Eigen::MatrixXd X_True(2, 1);
////    X_True << 0,
////            1.0;
////
////    Eigen::MatrixXd w(2, 1);
////    w << Gaussian_noise(0, sqrt(inputQ(0, 0))),
////            Gaussian_noise(0, sqrt(inputQ(1, 1)));
////
////    Eigen::MatrixXd v(2, 1);
////
////    Eigen::MatrixXd inputZ(2, 1);
//
//    int n = 2, l = 0, m = 2;
//
//    double T = 1.0;
//
//    Eigen::MatrixXd p0 = Eigen::MatrixXd(n, n);
//    p0 << 1, 0,
//            0, 1;
//
//    Eigen::MatrixXd x_Hat0 = Eigen::MatrixXd(n, 1);
//    x_Hat0 << 0.0,
//            1.0;
//
//    Eigen::MatrixXd inputQ = Eigen::MatrixXd(n, n);
//    inputQ << 0, 0,
//            0, 1;
//
//    Eigen::MatrixXd inputR = Eigen::MatrixXd(n, n);
//    inputR << 1, 0,
//            0, 1;
//
//    Eigen::MatrixXd inputA = Eigen::MatrixXd(n, n);
//    inputA << 1.0, T,
//            0, 1.0;
//
//    Eigen::MatrixXd inputH = Eigen::MatrixXd(m, n);
//    inputH << 1.0, 0,
//            0, 1.0;
//
//    Eigen::MatrixXd Kk0 = Eigen::MatrixXd(n, n);
//
//    Eigen::MatrixXd inputI = Eigen::MatrixXd::Identity(n, n);
//
//    kf.Init(p0, x_Hat0, inputQ, inputR, inputA, inputH, inputI, Kk0);
//
//    Eigen::MatrixXd X_True(2, 1);
//    X_True << 0,
//            20.0;
//
//    Eigen::MatrixXd w(1, 1);
//    w << Gaussian_noise(0, sqrt(inputQ(1, 1)));
//
//    Eigen::MatrixXd v(2, 1);
//
//    Eigen::MatrixXd inputZ(2, 1);
//
//    Eigen::MatrixXd Para(2,1);
//    Para << T*T/2,
//            T;
//
//    //=============保存数据用
//    std::vector<double> X1_True, X2_True;
//    std::vector<double> X1_MEA, X2_MEA;
//
//    for (int i = 0; i < 100; ++i) {
////        X_True = inputA * X_True + w;
////
////        w << Gaussian_noise(0, sqrt(inputQ(0, 0))),
////                Gaussian_noise(0, sqrt(inputQ(1, 1)));
////        v << Gaussian_noise(0, sqrt(inputR(0, 0))),
////                Gaussian_noise(0, sqrt(inputR(1, 1)));
////
////        X1_True.push_back(X_True(0, 0));
////        X2_True.push_back(X_True(1, 0));
////
////        inputZ = inputH * X_True + v;
////
////        X1_MEA.push_back(inputZ(0, 0));
////        X2_MEA.push_back(inputZ(1, 0));
////
////        kf.Predict();
////        kf.Correct(inputZ);
////        kf.UpdateP();
//        X_True = inputA * X_True + Para * w;
//        w << Gaussian_noise(0, sqrt(inputQ(1, 1)));
//
//        X1_True.push_back(X_True(0, 0));
//        X2_True.push_back(X_True(1, 0));
//
//        X1_MEA.push_back(X_True(0, 0));
//        X2_MEA.push_back(X_True(1, 0));
//
//        kf.Predict();
//        kf.Correct(X_True);
//        kf.UpdateP();
//    }
//
//
//
//    //=============保存数据
//    std::ofstream out("../out.txt");
//    auto it = X1_True.begin();
//    if (out.is_open()) {
//        std::cout << "Open" << std::endl;
//        while (it != X1_True.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        it = X2_True.begin();
//        while (it != X2_True.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        it = X1_MEA.begin();
//        while (it != X1_MEA.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        it = X2_MEA.begin();
//        while (it != X2_MEA.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X1_P_Hat = kf.GetX1_Prior_Hat();
//        it = X1_P_Hat.begin();
//        while (it != X1_P_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X2_P_Hat = kf.GetX2_Prior_Hat();
//        it = X2_P_Hat.begin();
//        while (it != X2_P_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X1_Hat = kf.GetX1_Hat();
//        it = X1_Hat.begin();
//        while (it != X1_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X2_Hat = kf.GetX2_Hat();
//        it = X2_Hat.begin();
//        while (it != X2_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        out.close();
//    }
//    std::cout << "Out\n" << std::endl;
//    return 0;
//}
//
//// mean:期望， stddev:标准差
//double Gaussian_noise(double mean, double stddev) {
//    std::default_random_engine generator(std::chrono::system_clock::now().time_since_epoch().count());
//    std::normal_distribution<double> dist(mean, stddev);
//    return dist(generator);
//}